Localization
First we have a look at the observation data:
Here we could know that it got 18 distance data each time, 20 degrees for each, and the unit should be meter.
Then we implemented the function to calculate the operation between two positions, return its yaw_start, distance and yaw_end.
The next step is to calculating the probability of getting the cur_pose that start from the same prev_pose, given input u. The code is shown below:
Then we update the probabilities in loc.bel_bar based on loc.bel from the previous time step and the odometry motion model.
We need to notice that it is important to filter out the points with relatively low probabilities, otherwise it will stocks the computing progress and takes plenty of time.
Then we compute the final probability:
